import java.io.DataInputStream;
import java.io.DataOutputStream;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.util.ArrayList;
import java.util.List;
import java.util.Queue;
import java.util.Stack;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.comm.BTConnection;
import lejos.nxt.comm.Bluetooth;
import lejos.robotics.RegulatedMotor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.util.PilotProps;

public class robot {
	public static void main(String[] aArg) throws IOException,
			InterruptedException {

		PilotProps pp = new PilotProps();
		pp.loadPersistentValues();

		float wheelDiameter = Float.parseFloat(pp.getProperty(
				PilotProps.KEY_WHEELDIAMETER, "5.48"));
		float trackWidth = Float.parseFloat(pp.getProperty(
				PilotProps.KEY_TRACKWIDTH, "13.61"));
		RegulatedMotor leftMotor = PilotProps.getMotor(pp.getProperty(
				PilotProps.KEY_LEFTMOTOR, "B"));
		RegulatedMotor rightMotor = PilotProps.getMotor(pp.getProperty(
				PilotProps.KEY_RIGHTMOTOR, "C"));
		boolean reverse = Boolean.parseBoolean(pp.getProperty(
				PilotProps.KEY_REVERSE, "false"));
		DifferentialPilot robot = new DifferentialPilot(wheelDiameter,
				trackWidth, leftMotor, rightMotor, reverse);

		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		LightSensor ls = new LightSensor(SensorPort.S2);

		robot.setAcceleration(30);
		robot.setRotateSpeed(20);
		robot.setTravelSpeed(30);
		LCD.clear();
		LCD.drawString("maze", 0, 1);
		LCD.refresh();

		Button.waitForPress();
		Thread.sleep(2000);
		// size of graph: 11*11
		int sizeX = 12;
		int sizeY = 12;
		// // initialize graph, set its size, set its nodes
		Graph firstFloor = new Graph(sizeY, sizeX);
		// // set the graph for each node
		firstFloor.setNodesOfGraph();
		// // initialize start point
		int startX = 6;
		int startY = 6;
		firstFloor.setStartX(startX);
		firstFloor.setStartY(startY);
		Node startNode = firstFloor.nodes[firstFloor.getStartY()][firstFloor
				.getStartX()];
		// Thread.sleep(2000);
		// // depth first search for solving first floor maze
		depthFirstSearch(robot, us, ls, firstFloor, startNode, 0);
		findDoors(firstFloor);
		exploreDoors(robot, firstFloor);

	}

	/*
	 * depthFirstSearch(DifferentialPilot robot, UltrasonicSensor us,
	 * LightSensor ls, Graph graph, Node startNode, int orientation)
	 * visitedNodes: a list which stores which node is visited colourfulNodes: a
	 * list which stores which node is colourful stackOfNodes: a stack which
	 * stores the path of currently visited nodes peek the current node at the
	 * top of the stack push the current node the stack mark the current node as
	 * visited add the current node to the visitedNodes array find the best next
	 * node of the current node from its edges, the priority is right < up <
	 * down <left draw the edges of the current node check whether the current
	 * node is colourful or not if the edges of the current node were visited,
	 * it is done, take it from the stack and back to the previous node at the
	 * stack if the stack is empty, all accessable nodes were visited the
	 * algorithm finishes and visited nodes and colourful nodes are set into the
	 * graph
	 */

	public static void depthFirstSearch(DifferentialPilot robot,
			UltrasonicSensor us, LightSensor ls, Graph graph, Node startNode,
			int orientation) throws InterruptedException, IOException {

		List<Node> visitedNodes = new ArrayList<Node>();
		List<Node> colourfulNodes = new ArrayList<Node>();
		Stack<Node> stackOfNodes = new Stack<Node>();
		stackOfNodes.push(startNode);
		startNode.setOrientation(orientation);

		String connected = "Connected";
		String waiting = "Waiting";
		LCD.clear();
		LCD.drawString(waiting, 0, 0);
		LCD.refresh();
		BTConnection btc = Bluetooth.waitForConnection();
		LCD.clear();
		LCD.drawString(connected, 0, 0);
		LCD.refresh();

		while (!(stackOfNodes.isEmpty())) {
			Node currentNode = stackOfNodes.peek();
			Node nextNode = null;
			if (!currentNode.isVisited()) {
				nextNode = findTheBestDirection(us, currentNode);
				currentNode.setVisited(true);
				visitedNodes.add(currentNode);
				drawEdges(currentNode);
				boolean isColour = findLight(ls, currentNode);
				if (isColour) {
					currentNode.setColour(isColour);
					if (colourfulNodes.size() < 4) {
						colourfulNodes.add(currentNode);
						Sound.beep();
					}
				}
			} else {
				nextNode = getBestEdge(currentNode, currentNode.up,
						currentNode.left, currentNode.down, currentNode.right);
			}

			sendDataToHost(currentNode, btc);

			if (!(nextNode == null)) {
				// direction of the next node according to the current node
				int direction = currentNode.getBestEdgeOfNode();
				goToNode(robot, direction);
				// push the node the stack
				stackOfNodes.push(nextNode);
				LCD.clear();
				System.out.println("Go to");
				System.out.println(nextNode.toString());
				LCD.refresh();
			} else {
				Node removedNode = stackOfNodes.pop();
				if (stackOfNodes.isEmpty()) {
					graph.setVisitedNodes(visitedNodes);
					graph.setColourfulNodes(colourfulNodes);
					System.out.println("Over");
					break;
				}
				Node backNode = stackOfNodes.peek();
				int difference = removedNode.orientation - backNode.orientation;
				backToNode(robot, difference);

				LCD.clear();
				System.out.println("Back to");
				System.out.println(backNode.toString());
				LCD.refresh();
			}
		}
	}

	/*
	 * getBestEdge(Node currentNode, int up, int left, int down, int right) take
	 * the free directions of the current node calculate the coordinates of the
	 * nodes at the free directions set them into the list of edges of the
	 * current node the priority level left > down > up> right choose the next
	 * node which is unvisited at the best free direction and according to
	 * priority level return it
	 */
	public static Node getBestEdge(Node currentNode, int up, int left,
			int down, int right) throws InterruptedException {
		// create edge between the node and the corresponding free direction
		Node bestNode = null;
		int newOrientation = 4;
		currentNode.up = up;
		currentNode.left = left;
		currentNode.down = down;
		currentNode.right = right;

		if (up == 1) {
			newOrientation = findNewOrientation(currentNode.orientation, 0);
			Node nextNode = currentNode.setEdges(newOrientation);
			if (!(nextNode.visited)) {
				bestNode = nextNode;
				bestNode.setOrientation(newOrientation);
				currentNode.setBestEdgeOfNode(0);
			}
		}
		if (right == 1) {
			newOrientation = findNewOrientation(currentNode.orientation, 1);
			Node nextNode = currentNode.setEdges(newOrientation);
			if (!(nextNode.visited)) {
				bestNode = nextNode;
				bestNode.setOrientation(newOrientation);
				currentNode.setBestEdgeOfNode(1);
			}
		}

		if (down == 1) {
			newOrientation = findNewOrientation(currentNode.orientation, 2);
			Node nextNode = currentNode.setEdges(newOrientation);
			if (!(nextNode.visited)) {
				bestNode = nextNode;
				bestNode.setOrientation(newOrientation);
				currentNode.setBestEdgeOfNode(2);
			}
		}
		if (left == 1) {
			newOrientation = findNewOrientation(currentNode.orientation, 3);
			Node nextNode = currentNode.setEdges(newOrientation);
			if (!(nextNode.visited)) {
				bestNode = nextNode;
				bestNode.setOrientation(newOrientation);
				currentNode.setBestEdgeOfNode(3);
			}
		}

		return bestNode;

	}

	/*
	 * findNewOrientation(int orientation, int direction) take the current
	 * orientation and the direction which will be gone calculate and return the
	 * new orientation
	 */
	public static int findNewOrientation(int orientation, int direction) {
		return ((orientation + direction) % 4);
	}

	/*
	 * findNewDirection(int newOrientation, int oldOrientation) take the
	 * orientation of the current node take the orientation of the next node
	 * which will be visited calculate the direction at which the robot should
	 * travel in order to go from the current to the next return the direction
	 */
	public static int findNewDirection(int newOrientation, int oldOrientation) {
		int m = 4;
		int n = newOrientation - oldOrientation;
		int dir = (n < 0) ? (m - (Math.abs(n) % m)) % m : (n % m);
		return dir;
	}

	/*
	 * findDirectionBetweenNodeToNode(Node currentNode, Node nextNode) take the
	 * position of the current node take the position of the next node which
	 * will be visited calculate the orientations of the nodes according to each
	 * other return the direction
	 */
	public static int findDirectionBetweenNodeToNode(Node currentNode,
			Node nextNode) {
		int oldOrientation = currentNode.getOrientation();
		int differenceX = nextNode.getX() - currentNode.getX();
		int differenceY = nextNode.getY() - currentNode.getY();
		int newOrientation = 0;
		if (differenceX == 1 && differenceY == 0) {
			newOrientation = 1;
		}
		if (differenceX == -1 && differenceY == 0) {
			newOrientation = 3;
		}
		if (differenceX == 0 && differenceY == -1) {
			newOrientation = 2;
		}
		if (differenceX == 0 && differenceY == 1) {
			newOrientation = 0;
		}
		int direction = findNewDirection(newOrientation, oldOrientation);
		nextNode.setOrientation(newOrientation);
		return direction;
	}

	/*
	 * findShortestPath(Node source, Node target) returns boolean if the path
	 * between the source and the target is found or not queueNodes: queue of
	 * the candidate nodes (First In First Out) observedNodes: a list of the
	 * checked nodes add the source to observedNodes add the source to
	 * queueNodes enqueue the node from the queue if the dequeued node is
	 * target, target is found and return true ow check the edges of the
	 * dequeued node if the edge of the dequeued node is not checked, add the
	 * edge to the queue set the parent of the edge of dequeued node as the
	 * dequeued node
	 */

	public static boolean findShortestPath(Node source, Node target)
			throws InterruptedException {
		boolean isPathFound = false;
		Queue<Node> queueNodes = new Queue<Node>();
		List<Node> observedNodes = new ArrayList<Node>();
		observedNodes.add(source);
		queueNodes.addElement(source);
		while (!queueNodes.isEmpty()) {
			Node dequeuedNode = queueNodes.elementAt(0);
			queueNodes.removeElement(dequeuedNode);
			if (dequeuedNode == target) {
				isPathFound = true;
				break;
			}

			Node edgeOfDequeuedNode = new Node();
			for (int i = 0; i < dequeuedNode.getEdges().size(); i++) {
				edgeOfDequeuedNode = dequeuedNode.getEdges().get(i);
				if (!(edgeOfDequeuedNode == null)) {
					if (!(observedNodes.contains(edgeOfDequeuedNode))) {
						observedNodes.add(edgeOfDequeuedNode);
						queueNodes.addElement(edgeOfDequeuedNode);
						edgeOfDequeuedNode.setParentNode(dequeuedNode);
					}
				}

			}
		}
		return isPathFound;
	}

	/*
	 * shortestPath(Node source, Node target) returns the shortest path
	 * shortestPath:the path between the source and the target add the node to
	 * the shortest path add the parent of the node to the shortest path if the
	 * parent is the source, the path from target to the source is found returns
	 * the reversed of the found path (from the source to the target)
	 */
	public static List<Node> shortestPath(Node source, Node target)
			throws InterruptedException {
		List<Node> shortestPath = new ArrayList<Node>();
		shortestPath.add(target);
		Node parent = target.getParentNode();
		while (!(source == parent)) {
			shortestPath.add(parent);
			parent = parent.getParentNode();
		}
		shortestPath.add(source);
		List<Node> reversedShortestPath = new ArrayList<Node>();
		reversedShortestPath = reverse(shortestPath);
		return reversedShortestPath;
	}

	public static List<Node> reverse(List<Node> shortestPath) {
		if (shortestPath.size() > 1) {
			Node node = shortestPath.remove(0);
			reverse(shortestPath);
			shortestPath.add(node);
		}
		return shortestPath;
	}

	/*
	 * findDoors(Graph graph) returns the list of doors after the maze in graph
	 * is solved, take the visited nodes calculate minimum x coordinate of the
	 * visited nodes calculate minimum y coordinate of the visited nodes
	 * calculate maximum x coordinate of the visited nodes calculate maximum y
	 * coordinate of the visited nodes if the differences between the min and
	 * max coordinates is 3, the doors stand in these coordinates
	 */
	public static void findDoors(Graph graph) {
		List<Node> visitedNodes = graph.getVisitedNodes();
		List<Node> doorsNodes = new ArrayList<Node>();
		int minX = graph.getSizeX() - 1;
		int maxX = 0;
		int minY = graph.getSizeY() - 1;
		int maxY = 0;

		int x;
		int y;
		for (int i = 0; i < visitedNodes.size(); i++) {
			Node node = visitedNodes.get(i);
			x = node.getX();
			y = node.getY();
			if (x < minX)
				minX = x;
			if (x > maxX)
				maxX = x;
			if (y < minY)
				minY = y;
			if (y > maxY)
				maxY = y;
		}
		if (maxX - minX == 3) {
			if (minY == graph.getStartY()) {
				for (int i = minX; i <= maxX; i++) {
					doorsNodes.add(graph.nodes[maxY][i]);
				}
			}
			if (maxY == graph.getStartY()) {
				for (int i = minX; i <= maxX; i++) {
					doorsNodes.add(graph.nodes[minY][i]);
				}
			}

		}
		if (maxY - minY == 3) {
			if (minX == graph.getStartX()) {
				for (int i = minY; i <= maxY; i++) {
					doorsNodes.add(graph.nodes[i][maxX]);
				}
			}
			if (maxX == graph.getStartX()) {
				for (int i = minY; i <= maxY; i++) {
					doorsNodes.add(graph.nodes[i][minX]);
				}
			}

		}

		graph.setDoorsNodes(doorsNodes);
	}

	/*
	 * exploreDoors(DifferentialPilot robot, Graph graph) explore what happens
	 * after the doors the robot is at the start position and first combination
	 * of the colourful nodes are done go to the doors and explore the doors
	 * sequentially if the ramp is not found, try another combination if the
	 * ramp is found, break the function and return to the start position
	 */
	public static void exploreDoors(DifferentialPilot robot, Graph graph)
			throws InterruptedException, IOException {
		List<Node> colourfulNodes = new ArrayList<Node>();
		colourfulNodes = graph.getColourfulNodes();
		List<Node> doorsNodes = new ArrayList<Node>();
		doorsNodes = graph.getDoorsNodes();
		boolean isCompleted = false;
		Node startNode = graph.nodes[graph.getStartY()][graph.getStartX()];
		System.out.println("Combination1 : Patch1-Patch2-Patch3");
		Sound.beep();
		int numberOfCombinations = 2;
		int[] sequence = combinationSequence(numberOfCombinations);
		int i = sequence[0];
		int j = sequence[1];
		int k = sequence[2];
		int doorNumber;
		while (numberOfCombinations < 7) {
			isCompleted = followPathFromPatchToDoor(robot, startNode,
					doorsNodes);
			doorNumber = graph.getDoorNumber();
			if (isCompleted == false) {
				combination(robot, doorsNodes.get(doorNumber),
						colourfulNodes.get(i), colourfulNodes.get(j),
						colourfulNodes.get(k));

			} else {
				findShortestPath(doorsNodes.get(doorNumber), startNode);
				List<Node> pathFromDoorToStart = shortestPath(
						doorsNodes.get(doorNumber), startNode);
				followTheShortestPath(robot, pathFromDoorToStart, doorsNodes
						.get(doorNumber).getOrientation());
				break;
			}
			startNode = colourfulNodes.get(k);
			numberOfCombinations++;
			sequence = combinationSequence(numberOfCombinations);
			i = sequence[0];
			j = sequence[1];
			k = sequence[2];
		}
	}

	/*
	 * combination(DifferentialPilot robot, Node initialNode,Node colour1Node,
	 * Node colour2Node, Node colour3Node) follow path from the final position
	 * to the colour patch1 follow path from the colour patch 1 to the colour
	 * patch2 follow path from the colour patch 2 to the colour patch3
	 */
	public static void combination(DifferentialPilot robot, Node initialNode,
			Node colour1Node, Node colour2Node, Node colour3Node)
			throws InterruptedException {
		boolean isPathFound = false;
		isPathFound = findShortestPath(initialNode, colour1Node);
		if (isPathFound) {
			List<Node> pathFromStartToColour1 = shortestPath(initialNode,
					colour1Node);
			followTheShortestPath(robot, pathFromStartToColour1,
					initialNode.getOrientation());
			Sound.beep();
		}
		isPathFound = findShortestPath(colour1Node, colour2Node);
		if (isPathFound) {
			List<Node> pathFromColour1ToColour2 = shortestPath(colour1Node,
					colour2Node);
			followTheShortestPath(robot, pathFromColour1ToColour2,
					colour1Node.getOrientation());
			Sound.beep();
		}
		isPathFound = findShortestPath(colour2Node, colour3Node);
		if (isPathFound) {
			List<Node> pathFromColour2ToColour3 = shortestPath(colour2Node,
					colour3Node);
			followTheShortestPath(robot, pathFromColour2ToColour3,
					colour2Node.getOrientation());
			Sound.beep();
		}

	}

	/*
	 * combinationSequence(int numberOfCombination) according to the number of
	 * combination choose a combination of the colour patches
	 */
	public static int[] combinationSequence(int numberOfCombination) {
		int[] combination = new int[3];
		int i = 0, j = 0, k = 0;
		switch (numberOfCombination) {
		case 1:
			i = 0;
			j = 1;
			k = 2;
			break;
		case 2:
			i = 1;
			j = 2;
			k = 0;
			break;
		case 3:
			i = 2;
			j = 0;
			k = 1;
			break;
		case 4:
			i = 0;
			j = 2;
			k = 1;
			break;
		case 5:
			i = 1;
			j = 0;
			k = 2;
			break;
		case 6:
			i = 2;
			j = 1;
			k = 0;
			break;
		}
		combination[0] = i;
		combination[1] = j;
		combination[2] = k;
		return combination;
	}

	/*
	 * followPathFromPatchToDoor(DifferentialPilot robot, Node
	 * initialNode,List<Node> doorsNodes) if the combination of the colour
	 * patches is done, follow the path from the final colour patch to the door1
	 * if the door is open, follow the path after the door if the ramp is found,
	 * return true and set the door number which has already explored ow return
	 * false and set the door number which has already explored ow all doors are
	 * closed, returns false and set the door number as 3 which has already
	 * explored
	 */
	public static boolean followPathFromPatchToDoor(DifferentialPilot robot,
			Node initialNode, List<Node> doorsNodes)
			throws InterruptedException, IOException {
		boolean isPathFound = false;
		boolean isRampFound = false;
		isPathFound = findShortestPath(initialNode, doorsNodes.get(0));
		List<Node> pathFromStartToDoor1 = shortestPath(initialNode,
				doorsNodes.get(0));
		followTheShortestPath(robot, pathFromStartToDoor1,
				initialNode.getOrientation());
		if (isDoorOpen(doorsNodes.get(0))) {
			isRampFound = followPathAfterDoor(robot, doorsNodes.get(0));
			doorsNodes.get(0).getGraph().setDoorNumber(0);
			return isRampFound;
		}
		isPathFound = findShortestPath(doorsNodes.get(0), doorsNodes.get(1));
		List<Node> pathFromDoor1ToDoor2 = shortestPath(doorsNodes.get(0),
				doorsNodes.get(1));
		followTheShortestPath(robot, pathFromDoor1ToDoor2, doorsNodes.get(0)
				.getOrientation());
		if (isDoorOpen(doorsNodes.get(1))) {
			isRampFound = followPathAfterDoor(robot, doorsNodes.get(1));
			doorsNodes.get(1).getGraph().setDoorNumber(1);
			return isRampFound;
		}
		isPathFound = findShortestPath(doorsNodes.get(1), doorsNodes.get(2));
		List<Node> pathFromDoor2ToDoor3 = shortestPath(doorsNodes.get(1),
				doorsNodes.get(2));
		followTheShortestPath(robot, pathFromDoor2ToDoor3, doorsNodes.get(1)
				.getOrientation());
		if (isDoorOpen(doorsNodes.get(2))) {
			isRampFound = followPathAfterDoor(robot, doorsNodes.get(2));
			doorsNodes.get(2).getGraph().setDoorNumber(2);
			return isRampFound;
		}
		isPathFound = findShortestPath(doorsNodes.get(2), doorsNodes.get(3));
		List<Node> pathFromDoor3ToDoor4 = shortestPath(doorsNodes.get(2),
				doorsNodes.get(3));
		followTheShortestPath(robot, pathFromDoor3ToDoor4, doorsNodes.get(2)
				.getOrientation());
		if (isDoorOpen(doorsNodes.get(3))) {
			isRampFound = followPathAfterDoor(robot, doorsNodes.get(3));
			doorsNodes.get(3).getGraph().setDoorNumber(3);
			return isRampFound;
		}
		doorsNodes.get(3).getGraph().setDoorNumber(3);
		return isRampFound;
	}

	/*
	 * findTheBestDirection(UltrasonicSensor us, Node currentNode) check the all
	 * edges of the current node via ultrsonic sensor measure the distance to
	 * north wall rotate ultrasonic sensor to east of the current position of
	 * the robot measure the distance to east wall rotate ultrasonic sensor to
	 * south of the current position of the robot the distance to south wall
	 * rotate ultrasonic sensor to west of the current position of the robot the
	 * distance to west wall rotate ultrasonic sensor to the initial position of
	 * the robot if the distance is bigger than the threshold distance which is
	 * measured as 25, it is free wall and set the wall as 1 create edge between
	 * the node and the corresponding free direction return the best free edge
	 */
	public static Node findTheBestDirection(UltrasonicSensor us,
			Node currentNode) throws InterruptedException {
		int thresholdUp = 25;
		int thresholdLeft = 25;
		int thresholdDown = 25;
		int thresholdRight = 25;

		int distanceUp = us.getDistance();
		Motor.A.rotate(90);
		int distanceRight = us.getDistance();
		Motor.A.rotate(90);
		int distanceDown = us.getDistance();
		Motor.A.rotate(-90);
		Motor.A.rotate(-90);
		Motor.A.rotate(-90);
		int distanceLeft = us.getDistance();
		Motor.A.rotate(90);

		int up = 0, left = 0, down = 0, right = 0;
		if (distanceUp > thresholdUp) {
			up = 1;
		}
		if (distanceLeft > thresholdLeft) {
			left = 1;
		}
		if (distanceDown > thresholdDown) {
			down = 1;
		}
		if (distanceRight > thresholdRight) {
			right = 1;
		}
		Node nextNode = getBestEdge(currentNode, up, left, down, right);
		return nextNode;
	}

	/*
	 * goToNode(DifferentialPilot robot, int direction) go forward rotate to the
	 * determined direction up is 0 right is 1 down is 2 left is 3 travel 30 cm
	 */
	public static void goToNode(DifferentialPilot robot, int direction)
			throws InterruptedException {
		if (direction == 3) {
			odometryCalibration(robot, direction);
			robot.rotate(90);
			LCD.clear();
			System.out.println("Rotate 90, left");
			LCD.refresh();
			Thread.sleep(1000);
		}
		if (direction == 0) {
			LCD.clear();
			System.out.println("forward");
			LCD.refresh();
			Thread.sleep(1000);
		}
		if (direction == 2) {
			robot.rotate(180);
			LCD.clear();
			System.out.println("backward");
			LCD.refresh();
			Thread.sleep(1000);
		}
		if (direction == 1) {
			odometryCalibration(robot, direction);
			robot.rotate(-90);
			LCD.clear();
			System.out.println("Rotate -90, right");
			LCD.refresh();
			Thread.sleep(1000);
		}
		robot.travel(30);
		Thread.sleep(2000);
	}

	/*
	 * backToNode(DifferentialPilot robot, int direction) go backward travel -30
	 * cm rotate according to the difference between the orientations of two
	 * nodes
	 */
	public static void backToNode(DifferentialPilot robot, int difference)
			throws InterruptedException {
		robot.travel(-30);
		if (difference == 3 || difference == -1) {
			// odometryCalibration(robot,1);
			robot.rotate(-90);
			LCD.clear();
			System.out.println("Rotate -90, right");
			LCD.refresh();
			Thread.sleep(1000);
		}
		if (difference == -3 || difference == 1) {
			// odometryCalibration(robot,3);
			robot.rotate(90);
			LCD.clear();
			System.out.println("Rotate 90, left");
			LCD.refresh();
			Thread.sleep(1000);
		}
		if (difference == 2 || difference == -2) {
			robot.rotate(180);
			LCD.clear();
			System.out.println("forward");
			LCD.refresh();
			Thread.sleep(1000);
		}
		if (difference == 0) {
			LCD.clear();
			System.out.println("backward");
			LCD.refresh();
			Thread.sleep(1000);
		}

	}

	public static void drawEdges(Node currentNode) throws InterruptedException {
		List<Node> edges = currentNode.getEdges();
		for (int i = 0; i < edges.size(); i++) {
			Node node = edges.get(i);
			int x = node.getX();
			int y = node.getY();

		}
	}

	public static boolean findLight(LightSensor ls, Node currentNode)
			throws InterruptedException {

		boolean isColour = false;
		if (ls.getNormalizedLightValue() < 300) {
			isColour = true;
		}
		if (isColour == true) {
			currentNode.setColour(isColour);
		}
		return isColour;
	}

	/*
	 * followTheShortestPath(DifferentialPilot robot, List<Node> path, int
	 * orientation) travel along the found shortest path take the orientation of
	 * the source node find the orientation of the next node find the direction
	 * of the next node to the previous node go to that direction
	 */
	public static void followTheShortestPath(DifferentialPilot robot,
			List<Node> path, int orientation) throws InterruptedException {
		path.get(0).setOrientation(orientation);
		for (int i = 0; i < path.size() - 1; i++) {
			int direction = findDirectionBetweenNodeToNode(path.get(i),
					path.get(i + 1));
			goToNode(robot, direction);
		}

	}

	/*
	 * isDoorOpen(Node doorNode) returns true if the door is opened check the
	 * door whether is open or not by rotating ultrasonic sensor if the the door
	 * is open, set the direction of the door inside the door node
	 */
	public static boolean isDoorOpen(Node doorNode) throws InterruptedException {
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		int nodeDirection = doorNode.getOrientation();
		int doorDirection = 0;
		int m = 4;
		int n = doorDirection - nodeDirection;
		int angle = (n < 0) ? (m - (Math.abs(n) % m)) % m : (n % m);
		int distance = 0;
		int threshold = 25;
		System.out.println(angle);
		Thread.sleep(4000);
		if (angle == 3) {
			Motor.A.rotate(-90);
			distance = us.getDistance();
			Motor.A.rotate(90);
			// LCD.clear();
			// System.out.println("Rotate 90, left");
			// LCD.refresh();
			// Thread.sleep(4000);
		}
		if (angle == 0) {
			distance = us.getDistance();
			// / LCD.clear();
			// System.out.println("forward");
			// LCD.refresh();
			// Thread.sleep(4000);
		}
		if (angle == 2) {
			Motor.A.rotate(180);
			distance = us.getDistance();
			Motor.A.rotate(-180);
			// LCD.clear();
			// System.out.println("backward");
			// LCD.refresh();
			// Thread.sleep(4000);
		}
		if (angle == 1) {
			Motor.A.rotate(90);
			distance = us.getDistance();
			Motor.A.rotate(-90);
			// LCD.clear();
			// System.out.println("Rotate -90, right");
			// LCD.refresh();
			// Thread.sleep(4000);
		}
		System.out.println(distance);
		Thread.sleep(4000);

		if (distance > threshold) {
			doorNode.setDoorDirection(angle);
			return true;
		}
		return false;

	}

	/*
	 * followPathAfterDoor(DifferentialPilot robot, Node node) returns true if
	 * the path is connected to the ramp rotate the robot to the door direction
	 * the path length is 5, go to five node rotate the ultrasonic sensor to
	 * right and measure the distance if the measured distance smaller then
	 * threshold, there is a wall and the path is not connected to the ramp go
	 * back to the door node ow there is no wall and the path is connected to
	 * the ramp go to the down start of the ramp climb the ramp explore the
	 * second floor and figure out the maze of the second floor back to the up
	 * start of the ramp climb down the ramp
	 */

	public static boolean followPathAfterDoor(DifferentialPilot robot, Node node)
			throws InterruptedException, IOException {
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		boolean isRampFound = false;
		int doorDirection = node.getDoorDirection();
		int newOrientation = ((node.getOrientation() + doorDirection) % 4);
		node.setOrientation(newOrientation);
		int threshold = 25;
		goToNode(robot, doorDirection);
		goToNode(robot, 0);
		goToNode(robot, 0);
		goToNode(robot, 0);
		goToNode(robot, 0);
		Motor.A.rotate(90);
		int distance = us.getDistance();
		Motor.A.rotate(-90);
		if (distance > threshold) {
			goToRamp(robot);
			climbRamp(robot);
			Thread.sleep(10000);
			secondFloor(robot);
			Thread.sleep(10000);
			climbDownRamp(robot);
			backFromRamp(robot);
			isRampFound = true;
		} else {
			robot.rotate(180);
			goToNode(robot, 0);
			goToNode(robot, 0);
			goToNode(robot, 0);
			goToNode(robot, 0);
			goToNode(robot, 0);

		}
		int newOrientation2 = ((node.getOrientation() + 2) % 4);
		node.setOrientation(newOrientation2);
		return isRampFound;
	}

	/*
	 * goToRamp(DifferentialPilot robot) go to the start of the ramp rotate the
	 * motor to right if there is no wall in front of the robot, go to the next
	 * node ow the robot is at the start of the ramp, rotate the robot to right
	 * (the start of the ramp)
	 */
	public static void goToRamp(DifferentialPilot robot)
			throws InterruptedException {
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		int enterDirection = 1;
		int threshold = 25;
		goToNode(robot, enterDirection);
		int distance = us.getDistance();
		while (distance > threshold) {
			goToNode(robot, 0);
		}
		odometryCalibration(robot, 3);
		robot.rotate(90);
	}

	public static void climbRamp(DifferentialPilot robot)
			throws InterruptedException {

		robot.travel(-250);
	}

	public static void climbDownRamp(DifferentialPilot robot)
			throws InterruptedException {

		robot.travel(250);
	}

	/*
	 * backFromRamp(DifferentialPilot robot) back from the start of the ramp to
	 * the door that has explored rotate the motor to left if there is no wall
	 * in front of the robot, go to the next node ow the robot is at the start
	 * of the path to the door, rotate the robot to left (the start of the ramp)
	 */
	public static void backFromRamp(DifferentialPilot robot)
			throws InterruptedException {
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		int exitDirection = 3;
		int threshold = 25;
		goToNode(robot, exitDirection);
		int distance = us.getDistance();
		while (distance > threshold) {
			goToNode(robot, 0);
		}
		goToNode(robot, exitDirection);
		goToNode(robot, 0);
		goToNode(robot, 0);
		goToNode(robot, 0);
		goToNode(robot, 0);
	}

	/*
	 * secondFloor(DifferentialPilot robot) figure out the map of the second
	 * floor maze the dimension of the second floor is 3*4 the dimension of map
	 * is 7*7 the position of the start node is x=3 and y=3 initialize the
	 * second floor graph depth first search for the second floor after the
	 * robot goes back the initial position, backward and rotate the robot to
	 * left
	 */
	public static void secondFloor(DifferentialPilot robot)
			throws InterruptedException, IOException {
		UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
		LightSensor ls = new LightSensor(SensorPort.S2);
		goToNode(robot, 3);
		int sizeX = 7;
		int sizeY = 7;
		Graph secondFloor = new Graph(sizeY, sizeX);
		secondFloor.setNodesOfGraph();
		int startX = 3;
		int startY = 3;
		secondFloor.setStartX(startX);
		secondFloor.setStartY(startY);
		Node startNode = secondFloor.nodes[secondFloor.getStartY()][secondFloor
				.getStartX()];
		// depth first search for solving second floor maze
		depthFirstSearch(robot, us, ls, secondFloor, startNode, 0);
		goToNode(robot, 2);
		odometryCalibration(robot, 3);
		robot.rotate(90);
		Sound.beep();
		Sound.beep();
		Sound.beep();
	}

	public static void odometryCalibration(DifferentialPilot robot,
			int direction) throws InterruptedException {

		if (direction == 3) {
			robot.arc(-2.00, -55.0);
			robot.travel(-4);
			robot.rotate(55);
			robot.travel(-3);
		}
		if (direction == 0) {

		}
		if (direction == 2) {
		}
		if (direction == 1) {
			robot.arc(2.00, 55.0);
			robot.travel(-4);
			robot.rotate(-55);
			robot.travel(-3);
		}
	}

	public static void sendDataToHost(Node currentNode, BTConnection btc)
			throws IOException {
		OutputStream outputs = btc.openOutputStream();
		DataOutputStream outputstr = new DataOutputStream(outputs);
		InputStream inputs = btc.openInputStream();
		DataInputStream inputstr = new DataInputStream(inputs);

		/* For Bluetooth Connection */
		boolean wallU = true, wallR = true, wallD = true, wallL = true;
		for (Node neigh : currentNode.edges) {
			if (((neigh.x - currentNode.x) > 0) && (neigh.y == currentNode.y)) {
				wallR = false;
			}
			if (((neigh.x - currentNode.x) < 0) && (neigh.y == currentNode.y)) {
				wallL = false;
			}
			if (((neigh.y - currentNode.y) > 0) && (neigh.x == currentNode.x)) {
				wallU = false;
			}
			if (((neigh.y - currentNode.y) < 0) && (neigh.x == currentNode.x)) {
				wallD = false;
			}

		}

		outputstr.writeInt(currentNode.x);
		outputstr.flush();
		outputstr.writeInt(currentNode.y);
		outputstr.flush();
		outputstr.writeBoolean(wallU);
		outputstr.flush();
		outputstr.writeBoolean(wallR);
		outputstr.flush();
		outputstr.writeBoolean(wallD);
		outputstr.flush();
		outputstr.writeBoolean(wallL);
		outputstr.flush();

		outputstr.writeBoolean(currentNode.colour);
		outputstr.flush();
	} // End of sendDataToHost()

}